#include "mathextend.h"

MathExtend::MathExtend(QObject *parent) : QObject(parent)
{

}



float MathExtend::fast_atan2(float y, float x)
{
    float x_abs, y_abs, z;
    float alpha, angle, base_angle;
    int index;

    /* don't divide by zero! */
    if ((y == 0.0f) && (x == 0.0f))
        angle = 0.0f;
    else
    {
        /* normalize to +/- 45 degree range */
        y_abs = ABS(y);
        x_abs = ABS(x);
        //z = (y_abs < x_abs ? y_abs / x_abs : x_abs / y_abs);
        if (y_abs < x_abs)
            z = y_abs / x_abs;
        else
            z = x_abs / y_abs;
        /* when ratio approaches the table resolution, the angle is */
        /*      best approximated with the argument itself...       */
        if (z < TAN_MAP_RES)
            base_angle = z;
        else
        {
            /* find index and interpolation value */
            alpha = z * (float) TAN_MAP_SIZE - .5f;
            index = (int) alpha;
            alpha -= (float) index;
            if(index<=255 && index>=0)
            {
                /* determine base angle based on quadrant and */
                /* add or subtract table value from base angle based on quadrant */
                base_angle = fast_atan_table[index];
                base_angle += (fast_atan_table[index + 1] - fast_atan_table[index]) * alpha;
            }
        }

        if (x_abs > y_abs)
        {        /* -45 -> 45 or 135 -> 225 */
            if (x >= 0.0f)
            {           /* -45 -> 45 */
                if (y >= 0.0f)
                    angle = base_angle;   /* 0 -> 45, angle OK */
                else
                    angle = -base_angle;  /* -45 -> 0, angle = -angle */
            }
            else
            {                  /* 135 -> 180 or 180 -> -135 */
                angle = 3.14159265358979323846;

                if (y >= 0.0f)
                    angle -= base_angle;  /* 135 -> 180, angle = 180 - angle */
                else
                    angle = base_angle - angle;   /* 180 -> -135, angle = angle - 180 */
            }
        }
        else
        {                    /* 45 -> 135 or -135 -> -45 */
            if (y >= 0.0f)
            {           /* 45 -> 135 */
                angle = 1.57079632679489661923;

                if (x >= 0.0f)
                    angle -= base_angle;  /* 45 -> 90, angle = 90 - angle */
                else
                    angle += base_angle;  /* 90 -> 135, angle = 90 + angle */
            }
            else
            {                  /* -135 -> -45 */
                angle = -1.57079632679489661923;

                if (x >= 0.0f)
                    angle += base_angle;  /* -90 -> -45, angle = -90 + angle */
                else
                    angle -= base_angle;  /* -135 -> -90, angle = -90 - angle */
            }
        }
    }


#ifdef ZERO_TO_TWOPI
    if (angle < 0)
        return (angle + TWOPI);
    else
        return (angle);
#else
  return (angle);
#endif
}


// 去除死区，两边数据向0点偏移
float MathExtend::dead_zone(float x, float zone)
{
    float out = 0;
    if		 (x > zone)  out = x-zone;
    else if(x <-zone)  out = x+zone;
  return (out);
}

// 直接去除死区
float MathExtend::deadzone_range(float x,float zoom)
{
    return (x> -zoom && x < zoom) ? 0 : x;
}

// 0~360转为+-PI
float MathExtend::wrap_around_PI(float x)
{
    return (x>M_PI?(x-TWO_PI):(x<-M_PI?(x+TWO_PI):x));
}

// +-PI转为0~TWO_PI
float MathExtend::wrap_around_2PI(float x)
{
    return (x>TWO_PI?(x-TWO_PI):(x<0?(x+TWO_PI):x));
}

/**
 * 方法								用时				精确度
 *	invSqrt						16837       小数后1~2位
 *	1/__sqrtf()				17211				小数后4~5位
 *	my_cos						25
 */
float MathExtend::invSqrt(float x)
{
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long*)&y;
    i = 0x5f3759df - (i>>1);
    y = *(float*)&i;
    y = y * (1.5f - (halfx * y * y));
    return y;
}

/**
 * 快速平方根算法，小数后7位准确。 __sqrtf() 小数后8位有效
 * 同样计算量，__sqrtf() 14410ms        fastsqrt()  18910ms
 */
float MathExtend::fastsqrt(float x)
{
    float halfx = 0.5f*x;
    float y = x;
    long i = * ( long * ) &y;
    i = 0x5f3759df - ( i >> 1 );
    y = * ( float * ) &i;
    y = y * ( 1.5f - ( halfx * y * y ) );
    y = y * ( 1.5f - ( halfx * y * y ) );
    return x * y;
}

/**
 * 方法								用时
 *	arm_sin_f32				1
 *	my_sin						25
 *	my_cos						25
 */
double MathExtend::mx_sin(double rad)
{
    double sine;
    if (rad < 0)
        sine = rad*(1.27323954f + 0.405284735f * rad);
    else
        sine = rad * (1.27323954f - 0.405284735f * rad);
    if (sine < 0)
        sine = sine*(-0.225f * (sine + 1) + 1);
    else
        sine = sine * (0.225f *( sine - 1) + 1);
    return sine;
}

double MathExtend::my_sin(double rad)
{
    int8_t flag = 1;

    if (rad >= M_PI)
    {
        rad -= M_PI;
        flag = -1;
    }

    return mx_sin(rad) * flag;
}

float MathExtend::my_cos(double rad)
{
    int8_t flag = 1;
    rad += M_PI/2.0;

    if (rad >= M_PI)
    {
        flag = -1;
        rad -= M_PI;
    }

    return my_sin(rad)*flag;
}


float MathExtend::safe_sqrt(float v)
{
    float ret = sqrtf(v);
    if (isnan(ret))  return 0;
    return ret;
}
